#!/usr/bin/env python3

import serial
import rospy
from sensor_msgs.msg import Joy

class JoyToSerial:
    def __init__(self):
        # 初始化节点
        rospy.init_node('joy_to_serial')
        # 配置串口参数
        self.ser = serial.Serial('/dev/ttyUSB0',115200,timeout=1)
        # 订阅手柄话题
        self.sub = rospy.Subscriber('joy',Joy,self.joy_callback)
    def map_value(self, value, from_min, from_max, to_min, to_max):
        # 线性映射公式
        return (value - from_min)*(to_max- to_min)/(from_max - from_min) + to_min
    def joy_callback(self,joy_msg):
        # 读取参数
        linear_guidewires = joy_msg.axes[4]
        linear_catheter = joy_msg.axes[1]
        angular_guidewires = joy_msg.axes[6]
        # 处理读取的参数
        # 转换为50mm/s
        linear_guidewires_mapped = self.map_value(linear_guidewires, -1.0, 1.0, -375.0, 375.0)
        # linear_catheter_mapped = self.map_value(linear_catheter, -32767.0, 32767.0, -375.0, 375.0)
        # angular_guidewires_mapped = self.map_value(angular_guidewires, -32767.0, 32767.0, -375.0, 375.0)
        
        # 将指令编码为字符串
        # date = f"{linear_guidewires_mapped:.2f},{linear_catheter_mapped:.2f},{angular_guidewires_mapped:.2f}\n".encode()
        date = f"{linear_guidewires_mapped:.2f}\n".encode()
        
        # 发送到串口
        self.ser.write(date)
if __name__ == '__main__':
    try:
        joy_to_serial = JoyToSerial()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass